#ifndef _PID_
#define _PID_
#include <GetSysTimeMs.h>
#include <SetLCD5Char.h>
#include <SetMotor_Code.h>
#include <GetMotorCode.h>
#include <SetMotorCode0.h>

#define MAX_PWM        255
#define LEFT 1
#define RIGHT 2


extern unsigned long nextPID ;

typedef struct {
  double TargetTicksPerFrame;  
  long Encoder;      
  long PrevEnc;     

 
  int PrevInput;         
  int ITerm;             
  long output;                   
}
SetPointInfo;

SetPointInfo leftPID, rightPID;

double LKp = 35.0;
double LKd = 30;
double LKi = 2.3;
double LKo =33;


double RKp = 35.0;
double RKd = 30;
double RKi = 2.3;
double RKo =33;



unsigned char moving = 0; 
int m_lsp=0;
int m_rsp=0;
void motorsWrite2(int Lsp,int Rsp)
{

	if(Lsp>=0) SetMotor_Code(1, 0, Lsp/2.55);
	else SetMotor_Code(1, 2,- Lsp/2.55);
	
	if(Rsp>=0) SetMotor_Code(2, 0, Rsp/2.55);
	else SetMotor_Code(2, 2,- Rsp/2.55);
}


void resetPID(){
   leftPID.TargetTicksPerFrame = 0.0;
   leftPID.Encoder = abs(GetMotorCode(LEFT));
   leftPID.PrevEnc = leftPID.Encoder;
   leftPID.output = 0;
   leftPID.PrevInput = 0;
   leftPID.ITerm = 0;

   rightPID.TargetTicksPerFrame = 0.0;
   rightPID.Encoder = abs(GetMotorCode(RIGHT));
   rightPID.PrevEnc = rightPID.Encoder;
   rightPID.output = 0;
   rightPID.PrevInput = 0;
   rightPID.ITerm = 0;
}

void doPID(SetPointInfo * p) {
  long Perror;
  long output;
  int input;
  if(m_lsp>=0)
    input = p->Encoder - p->PrevEnc;
  else
    input = -(p->Encoder - p->PrevEnc);
  
  Perror = p->TargetTicksPerFrame - input;

  output = (LKp * Perror -LKd * (input - p->PrevInput) + p->ITerm) / LKo;
  p->PrevEnc = p->Encoder;

  output += p->output;

  if (output >= MAX_PWM)
    output = MAX_PWM;
  else if (output <= -MAX_PWM)
    output = -MAX_PWM;
  else
    p->ITerm += LKi * Perror;

  

  p->output = output;
  p->PrevInput = input;
}
void doRPID(SetPointInfo * p) {
  long Perror;
  long output;
  int input;
  
  if(m_rsp>=0)
    {
      input = p->Encoder - p->PrevEnc;
    }
  else
    {
      input = -(p->Encoder - p->PrevEnc);
    }
  Perror = p->TargetTicksPerFrame - input;
  

  output = (RKp * Perror - RKd * (input - p->PrevInput) + p->ITerm) / RKo;
  p->PrevEnc = p->Encoder;

  output += p->output;

  if (output >= MAX_PWM)
    output = MAX_PWM;
  else if (output <= -MAX_PWM)
    output = -MAX_PWM;
  else
    p->ITerm += RKi * Perror;

  

  p->output = output;
  p->PrevInput = input;
}
void updatePID() {
  /* Read the encoders */
  leftPID.Encoder = abs(GetMotorCode(LEFT));
  rightPID.Encoder =  abs(GetMotorCode(RIGHT));
  

  if (!moving){

    if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0) resetPID();
    return;
  }
if(m_rsp!=0)
doRPID(&rightPID);
if(m_lsp!=0)
  doPID(&leftPID);
  
motorsWrite2(leftPID.output, rightPID.output);

}

long readPidIn(int left)
{
  long pidin = 0;
  if (left == LEFT)
  {
    pidin = leftPID.PrevInput;
  }
  else
  {  
    pidin = rightPID.PrevInput;
  }

  return pidin;
}

long readPidOut(int left)
{
  long pidout = 0;
  if (left == LEFT)
  {
    pidout = leftPID.output;
  }
  else
  {
    pidout = rightPID.output;
  }
  return pidout;
}

#endif

